#ifndef GAMEENGINE_GEOMETRY_ANGLE_HPP
#define GAMEENGINE_GEOMETRY_ANGLE_HPP

#include <GameEngine/Geometry/Location/vector3d.h>
#include <GameEngine/Geometry/Location/vector2d.h>

#include <math.h>

#ifdef GAMEENGINE_NAMESPACES
namespace GameEngine {
#endif

//=================================================================================
// Implementation of the Angle class

//! Default constructor
/*!
 @note Class T should have a default constructor
 */
template <class T>
Angle<T>::Angle() {}

//! Constructor from three coordinates
/*!
 @param angle The angle
 @param system The angle system of the given angle
 @note Class T should have a copy constructor
 @note Class T should have an assignment operator
 */
template <class T>
Angle<T>::Angle(const T& angle, const Angles::AngleSystem& system) : grad_(angle) {
	if ( system == Angles::Deg ) grad_ = Angle::deg_to_grad(angle);
	else if ( system == Angles::Rad ) grad_ = Angle::rad_to_grad(angle);
}

//! Copy constructor
/*!
 @param rhs The point to copy
 @note Class T should have a copy constructor
 */
template <class T>
Angle<T>::Angle(const Angle& rhs) : grad_(rhs.grad_) {}

//! Copy constructor
/*!
 @param rhs The point to copy
 @note Class T should have a copy constructor
 */
template <class T>
template <class R>
Angle<T>::Angle(const Angle<R>& rhs) : grad_((T)rhs.grad()) {}

//! Destructor
/*!
 @note The destructor of class T should be public
 */
template <class T>
Angle<T>::~Angle() {}

//! Assignment operator
/*!
 @param rhs The angle to copy
 @note Class T should have an assignment operator
 */
template <class T>
Angle<T>& Angle<T>::operator=(const Angle& rhs) {
	if ( &rhs != this ) {
		grad_ = rhs.grad_;
	}
	return *this;
}

//! Assignment operator
/*!
 @param rhs The angle to copy
 @note Class T should have an assignment operator
 @note Class R should have implicit conversion with class T
 */
template <class T>
template <class R>
Angle<T>& Angle<T>::operator=(const Angle<R>& rhs) {
	grad_ = (T)rhs.grad();
	return *this;
}

//! Addition operator
/*!
 @param rhs The second angle to use for addition
 */
template <class T>
Angle<T> Angle<T>::operator+(const Angle<T>& rhs) {
	return Angle<T>(*this).operator+=(rhs);
}

//! Addition operator
/*!
 @param rhs The second angle to use for addition
 */
template <class T>
template <class R>
Angle<T> Angle<T>::operator+(const Angle<R>& rhs) {
	return Angle<T>(*this).operator+=(rhs);
}

//! Addition operator
/*!
 @param rhs The angle to use for addition
 @note Class T should have an addition operator
 */
template <class T>
Angle<T>& Angle<T>::operator+=(const Angle& rhs) {
	grad_ += rhs.grad_;
	return *this;
}

//! Addition operator
/*!
 @param rhs The angle to use for addition
 @note Class T should have an addition operator
 @note Class R should have implicit conversion with class T
 */
template <class T>
template <class R>
Angle<T>& Angle<T>::operator+=(const Angle<R>& rhs) {
	grad_ += (T)rhs.grad();
	return *this;
}

//! Subtraction operator
/*!
 @param rhs The second angle to use for subtraction
 */
template <class T>
Angle<T> Angle<T>::operator-(const Angle<T>& rhs) {
	return Angle<T>(*this).operator-=(rhs);
}

//! Subtraction operator
/*!
 @param rhs The second angle to use for subtraction
 */
template <class T>
template <class R>
Angle<T> Angle<T>::operator-(const Angle<R>& rhs) {
	return Angle<T>(*this).operator-=(rhs);
}

//! Subtraction operator
/*!
 @param rhs The angle to use for subtraction
 @note Class T should have a subtraction operator
 */
template <class T>
Angle<T>& Angle<T>::operator-=(const Angle& rhs) {
	grad_ -= rhs.grad_;
	return *this;
}

//! Subtraction operator
/*!
 @param rhs The angle to use for subtraction
 @note Class T should have a subtraction operator
 @note Class R should have implicit conversion with class T
 */
template <class T>
template <class R>
Angle<T>& Angle<T>::operator-=(const Angle<R>& rhs) {
	grad_ -= (T)rhs.grad();
	return *this;
}

//! Multiplication operator
/*!
 @param rhs The value to use for multiplication
 @note Class T should have a multiplication operator
 */
template <class T>
Angle<T> Angle<T>::operator*(const T& rhs) {
	return Angle<T>(*this).operator*=(rhs);
}

//! Multiplication operator
/*!
 @param rhs The value to use for multiplication
 @note Class T should have a multiplication operator
 */
template <class T>
Angle<T>& Angle<T>::operator*=(const T& rhs) {
	grad_ *= rhs;
	return *this;
}

//! Division operator
/*!
 @param rhs The value to use for division
 @note Class T should have a division operator
 */
template <class T>
Angle<T> Angle<T>::operator/(const T& rhs) {
	return Angle<T>(*this).operator/=(rhs);
}

//! Division operator
/*!
 @param rhs The value to use for division
 @note Class T should have a division operator
 */
template <class T>
Angle<T>& Angle<T>::operator/=(const T& rhs) {
	grad_ /= rhs;
	return *this;
}

//! Inferior operator
/*!
 @param rhs The value to test
 */
template <class T>
bool Angle<T>::operator<(const Angle& rhs) const {
	return ( grad_ < rhs.grad_ );
}

//! Inferior or equal operator
/*!
 @param rhs The value to test
 */
template <class T>
bool Angle<T>::operator<=(const Angle& rhs) const {
	return ( grad_ <= rhs.grad_ );
}

//! Superior operator
/*!
 @param rhs The value to test
 */
template <class T>
bool Angle<T>::operator>(const Angle& rhs) const {
	return ( grad_ > rhs.grad_ );
}

//! Superior or equal operator
/*!
 @param rhs The value to test
 */
template <class T>
bool Angle<T>::operator>=(const Angle& rhs) const {
	return ( grad_ >= rhs.grad_ );
}

//! Comparison operator
/*!
 @param rhs The value to test
 */
template <class T>
bool Angle<T>::operator!=(const Angle& rhs) const {
	return ( grad_ != rhs.grad_ );
}

//! Comparison operator
/*!
 @param rhs The value to test
 */
template <class T>
bool Angle<T>::operator==(const Angle& rhs) const {
	return ( grad_ == rhs.grad_ );
}

//! Return the angle in given coordinate system
template <class T>
T Angle<T>::angle(const Angles::AngleSystem& system) const {
	if ( system == Angles::Deg ) return deg();
	else if ( system == Angles::Rad ) return rad();
	else return grad();
}

//! Return the angle in degrees
template <class T>
T Angle<T>::deg() const {
	return Angle::grad_to_deg(grad_);
}

//! Return the angle in radians
template <class T>
T Angle<T>::rad() const {
	return Angle::grad_to_rad(grad_);
}

//! Return the angle in grades
template <class T>
T Angle<T>::grad() const {
	return grad_;
}

//! Set the angle
/*!
 @param angle The angle to use
 @param system The angle system in which given angle is specified
 */
template <class T>
void Angle<T>::set_angle(const T& angle, const Angles::AngleSystem& system) {
	if ( system == Angles::Deg ) set_deg(angle);
	else if ( system == Angles::Rad ) set_rad(angle);
	else set_grad(angle);
}

//! Set the angle
/*!
 @param angle The angle to use, specified in degrees
 */
template <class T>
void Angle<T>::set_deg(const T& angle) {
	grad_ = Angle::deg_to_grad(angle);
}

//! Set the angle
/*!
 @param angle The angle to use, specified in radians
 */
template <class T>
void Angle<T>::set_rad(const T& angle) {
	grad_ = Angle::rad_to_grad(angle);
}

//! Set the angle
/*!
 @param angle The angle to use, specified in grades
 */
template <class T>
void Angle<T>::set_grad(const T& angle) {
	grad_ = angle;
}

//! Reduce the angle
/*!
 *  A reduced angle is in [0,360] degrees, or [0,400] grades,
 *  or [0,2 PI] radians.
 *
 @return A reference to this reduced angle.
 */
template <class T>
Angle<T>& Angle<T>::reduce_angle() {
	int tmp = (int)(grad_ / (T)400);
	grad_ -= (T)(400 * tmp);
	return *this;
}

//! Convert the given angle from grades to degrees
/*!
 @param grad_angle The angle to convert, specified in grades
 */
template <class T>
T Angle<T>::grad_to_deg(const T& grad_angle) {
	return (T)(0.9 * (double)grad_angle);
}

//! Convert the given angle from grades to radians
/*!
 @param grad_angle The angle to convert, specified in grades
 */
template <class T>
T Angle<T>::grad_to_rad(const T& grad_angle) {
	return (T)(PI * (double)grad_angle / 200.0);
}

//! Convert the given angle from radians to degrees
/*!
 @param rad_angle The angle to convert, specified in radians
 */
template <class T>
T Angle<T>::rad_to_deg(const T& rad_angle) {
	return (T)((double)rad_angle * 180.0 / PI);
}

//! Convert the given angle from radians to grades
/*!
 @param rad_angle The angle to convert, specified in radians
 */
template <class T>
T Angle<T>::rad_to_grad(const T& rad_angle) {
	return (T)(200.0 * (double)rad_angle / PI);
}

//! Convert the given angle from degrees to radians
/*!
 @param deg_angle The angle to convert, specified in degrees
 */
template <class T>
T Angle<T>::deg_to_rad(const T& deg_angle) {
	return (T)((double)deg_angle * PI / 180.0);
}

//! Convert the given angle from degrees to grades
/*!
 @param deg_angle The angle to convert, specified in degrees
 */
template <class T>
T Angle<T>::deg_to_grad(const T& deg_angle) {
	return (T)((double)deg_angle / 0.9);
}

//! Compute an angle from three 3D points
/*!
 @param p0 The first point
 @param p1 The second point
 @param p2 The third point
 @return The angle made by p0-p1-p2
 @note The angle computed from this method suffers from numerical accuracy issues and should
       not be used for purposes that require some precision.
 */
template <class T>
Angle<T> Angle<T>::from_points(const Point3d<T>& p0, const Point3d<T>& p1, const Point3d<T>& p2) {
	// Get normalized directions
	Vector3d<T> v1 = Vector3d<T>(p1, p0).normalize();
	Vector3d<T> v2 = Vector3d<T>(p1, p2).normalize();

	// Get dot product
	T cos_val = v1.dot(v2);

	// Get angle
	T a = acos(cos_val);
	return Angle<T>(a, Angles::Rad);
}

//! Compute an angle from three 2D points
/*!
 @param p0 The first point
 @param p1 The second point
 @param p2 The third point
 @return The angle made by p0-p1-p2
 @note The angle computed from this method suffers from numerical accuracy issues and should
       not be used for purposes that require some precision.
 */
template <class T>
Angle<T> Angle<T>::from_points(const Point2d<T>& p0, const Point2d<T>& p1, const Point2d<T>& p2) {
	// Get normalized directions
	Vector2d<T> v1 = Vector2d<T>(p1, p0).normalize();
	Vector2d<T> v2 = Vector2d<T>(p1, p2).normalize();

	// Get dot product
	T cos_val = v1.dot(v2);

	// Get angle
	T a = acos(cos_val);
	return Angle<T>(a, Angles::Rad);
}

#ifdef GAMEENGINE_NAMESPACES
} // namespace GameEngine
#endif

#endif
